package seg.appx;

import geom.Point;

import java.io.File;

import seg.appx.EllipseFit.ControlPoint;
import stat.Matrix;
import doc.SampledPoint;
import doc.Stroke;

public class Conic {

	private double a11, a12, a22, b1, b2;

	private double lambda, phi;

	private Point[] points;

	private Point spn, epn;

	public Matrix getA() {
		Matrix m = new Matrix(2,2);
		m.setValueAt(0, 0, getA11());
		m.setValueAt(1, 1, getA22());
		m.setValueAt(0, 1, getA12());
		m.setValueAt(1, 0, getA12());
		return m;
	}

	public Matrix getB() {
		Matrix m = new Matrix(2,1);
		m.setValueAt(0, 0, getB1());
		m.setValueAt(1, 0, getB2());
		return m;
	}

	public Matrix getAB() {
		Matrix m = new Matrix(3,3);
		m.setValueAt(0, 0, getA11());
		m.setValueAt(0, 1, getA12());
		m.setValueAt(0, 2, getB1());
		m.setValueAt(1, 0, getA12());
		m.setValueAt(1, 1, getA22());
		m.setValueAt(1, 2, getB2());
		m.setValueAt(2, 0, getB1());
		m.setValueAt(2, 1, getB2());
		m.setValueAt(2, 2, 1);
//		m.print();
		return m;
	}

	public double getA11() {
		return a11;
	}

	public void setA11(double a11) {
		this.a11 = a11;
	}

	public double getA12() {
		return a12;
	}

	public void setA12(double a12) {
		this.a12 = a12;
	}

	public double getA22() {
		return a22;
	}

	public void setA22(double a22) {
		this.a22 = a22;
	}

	public double getB1() {
		return b1;
	}

	public void setB1(double b1) {
		this.b1 = b1;
	}

	public double getB2() {
		return b2;
	}

	public void setB2(double b2) {
		this.b2 = b2;
	}

	public double getLambda() {
		return lambda;
	}

	public void setLambda(double lambda) {
		this.lambda = lambda;
	}

	public double getPhi() {
		return phi;
	}

	public void setPhi(double phi) {
		this.phi = phi;
	}

	public double calcFxy(double x, double y) {
		Matrix xy = new Matrix(1,3);
		xy.setValueAt(0, 0, x);
		xy.setValueAt(0, 1, y);
		xy.setValueAt(0, 2, 1);
		return calcFxy(xy);
	}

	public double calcFxy(Matrix xy) {
		return xy.multiply(getAB()).multiply(xy.transpose()).det();
	}

	public Point[] getPoints() {
		return points;
	}

	public void setPoints(Point[] ps) {
		points = ps;
	}

	public Point getSpn() {
//
//		return spn;
		return points[0];
	}

	public void setSpn(Point spn) {
		this.spn = spn;
	}

	public Point getEpn() {
//		return epn;
		return points[points.length-1];
	}

	public void setEpn(Point epn) {
		this.epn = epn;
	}

	private double x[][], y[][];

	private double xsum[], ysum[];

	private double xy[][];

	private double xs, xs2, ys, ys2, xe, xe2, ye, ye2;

	private void preprocess() {
		x = new double[4][];
		y = new double[4][];
		xsum = new double[4];
		ysum = new double[4];

		for (int i = 0; i < 4; i++) {
			xsum[i] = 0;
			ysum[i] = 0;
			x[i] = new double[getPoints().length];
			y[i] = new double[getPoints().length];

			for (int j = 0; j < x[i].length; j++) {
				if (i == 0) {
					x[i][j] = getPoints()[j].getX();
					y[i][j] = getPoints()[j].getY();
				} else {
					x[i][j] = x[0][j] * x[i-1][j];
					y[i][j] = y[0][j] * y[i-1][j];
				}
//				System.out.println("x at " + i + "," + j + " = " + x[i][j]);
//				System.out.println("y at " + i + "," + j + " = " + y[i][j]);
				xsum[i] += x[i][j];
				ysum[i] += y[i][j];
			}
		}

		xs = getSpn().getX(); xs2 = xs * xs;
		ys = getSpn().getY(); ys2 = ys * ys;
		xe = getEpn().getX(); xe2 = xe * xe;
		ye = getEpn().getY(); ye2 = ye * ye;

		xy = new double[3][3];
		for (int i = 0; i < getPoints().length; i++) {
			for (int xx = 0; xx < 3; xx++) {
				for (int yy = 0; yy < 3; yy++) {
					if (xx + yy > 2) {
						continue;
					}
					xy[xx][yy] += (x[xx][i] * y[yy][i]);
//					System.out.println("xy at " + xx + "," + yy + " = " + xy[xx][yy]);
				}
			}
		}

		A = new Matrix(7,7);
		A.setValues(new double[][] {
				new double[] {
						xsum[3],
						2*xy[2][0],
						xy[1][1],
						2*xsum[2],
						2*xy[1][0],
						.5*xs2,
						.5*xe2
				},
				new double[] {
						xy[2][0],
						2*xy[1][1],
						xy[0][2],
						2*xy[1][0],
						2*xy[0][1],
						xs*ys,
						xe*ye
				},
				new double[] {
						xy[1][1],
						2*xy[0][2],
						ysum[3],
						2*xy[0][1],
						2*ysum[2],
						.5*ys2,
						.5*ye2
				},
				new double[] {
						xsum[2],
						2*xy[1][0],
						xy[0][1],
						2*xsum[1],
						2*xy[0][0],
						xs,
						xe
				},
				new double[] {
						xy[1][0],
						2*xy[0][1],
						ysum[2],
						2*xy[0][0],
						2*ysum[1],
						ys,
						ye
				},
				new double[] {
						xs2,
						2*xs*ys,
						ys2,
						2*xs,
						2*ys,
						0,
						0
				},
				new double[] {
						xe2,
						2*xe*ye,
						ye2,
						2*xe,
						2*ye,
						0,
						0
				}
		});

		b = new Matrix(7,1);
		b.setValues(new double[][] {
				new double[] { xsum[1] },
				new double[] { xy[0][0] },
				new double[] { ysum[1] },
				new double[] { xsum[0] },
				new double[] { ysum[0] },
				new double[] { 1 },
				new double[] { 1 }
		});
		b = b.scale(-1);
	}

	private Matrix A;

	private Matrix b;

	public Matrix getESA() {
		if (A == null)
			preprocess();
		return A;
	}

	public Matrix getESB() {
		if (b == null)
			preprocess();
		return b;
	}

	public void calc(boolean us) {
//		preprocess();
//
//		LinearEquationSystem les = new LinearEquationSystem(getESA(), getESB());
//		les.calculate();
//
//		Matrix x = les.getX();
		double x[] = null;
		if (us) {
			Matrix m = getESA().inverse().multiply(getESB());
			x = m.getColOf(0);
		} else {
			EllipseFit ef = new EllipseFit();
			ControlPoint ps;
			for (int i = 0; i < points.length; i++) {
				ps = new ControlPoint((int)points[i].getX(), (int)points[i].getY());
				ef.addPoint(ps);
	//			ef.getParameters();
			}
			ef.calcParameters();
			x = ef.getParameters();//
		}
		a11 = x[0];
		a12 = x[1];
		a22 = x[2];
		b1 = x[3];
		b2 = x[4];
		System.out.println("a11=" + a11);
		System.out.println("a12=" + a12);
		System.out.println("a22=" + a22);
		System.out.println("b1=" + b1);
		System.out.println("b2=" + b2);

		I1 = a11 + a12;
		I2 = a11*a22 - a12*a12;

		Matrix tmp = new Matrix(3,3);
		tmp.setValues(new double[][] {
				new double[] { a11, a12, b1 },
				new double[] { a12, a22, b2 },
				new double[] { b1, b2, 1 }
		});
		I3 = tmp.det();

		System.out.println("I1=" + I1);
		System.out.println("I2=" + I2);
		System.out.println("I3=" + I3);

		double k = a11/a22;
		if (k > 2500 || k < 4E-4) {
			System.out.println("fit a line: k > 2500 or k < 4E-4");;
			return;
		}

		double xx = (a12 * b1 - a22 * b1) / I2;
		double yy = (a12 * b1 - a11 * b2) / I2;
		center = new Point(xx, yy);
		System.out.println("center=" + center);

		if (I2 > 0) {
			System.out.println("fit an ellipse: I2 > 0");

			double Xm = Math.sqrt(a22);
			double Ym = Math.sqrt(a11);

			double cos = - a12 / (Xm * Ym);
			double sin2 = (b1 * b1 / a11 + b2 * b2 / a22 - 1) / (a11 * a22);

			System.out.println("Xm=" + Xm);
			System.out.println("Ym=" + Ym);
			System.out.println("cos=" + cos);
			System.out.println("sin^2=" + sin2);
		} else {
			System.out.println("fit a hyperbola: I2 < 0");
		}



//		double delta = I1 * I1 - 4*I2;
//		if (delta > 0 && I2 > 0 && I1*I3 < 0) {
//			major = Math.sqrt(-2*I3 / (I2 * ( I1 - Math.sqrt(delta) ) ));
//			minor = Math.sqrt(-2*I3 / (I2 * ( I1 + Math.sqrt(delta) ) ));
//
//
//			double tan = (delta + a11- a22) / 2 / a12;
//			System.out.println("tan = " + tan);
//		} else {
//			if (delta < 0)
//			System.err.println("delta < 0");
//		}
	}

	public void print() {
		System.out.println("=====================================================");
		System.out.println("  center = " + center);
		System.out.println("  major  = " + major);
		System.out.println("  minor  = " + minor);
		System.out.println("  angle  = " + angle);
	}

	private double I1, I2, I3;

	private Point center;

	private double major;

	private double minor;

	private double angle;

	public Point getCenter() {
		return center;
	}

	public double getMajor() {
		return major;
	}

	public double getMinor() {
		return minor;
	}

	public double getAngle() {
		return angle;
	}

	public static void main(String[] args) {
		Conic c = new Conic();

		File file = new File("C:\\Workspace\\Exp\\segmentation\\sample\\tmp.seg.3");
		Stroke s = Stroke.getFromFile(file);
		s.preprocess();
		SampledPoint[] ps = s.getPoints();
		Point[] pp = new Point[ps.length];
		for (int i = 0; i < pp.length; i++) {
			pp[i] = ps[i].toPoint();
		}
		c.setPoints(pp);

//		c.getESA().print();
//		c.getESB().print();

//		c.getESA().inverse().multiply(c.getESB()).print();

//		System.out.println(c.getESA().det());

		c.calc(false);
//		c.print();

	}

}
